#ifndef CAR_CONTROLLER_H
#define CAR_CONTROLLER_H

#include "ros/ros.h"
#include <ros/time.h>
#include <ros/duration.h>
#include <math.h>
#include "std_msgs/Float64.h"
#include "geometry_msgs/Twist.h"
#include <algorithm>
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf/transform_broadcaster.h"

class CarController
{
public:
	CarController(){
		// int  argc  = 0;
    	// char *argv = nullptr;
		// ros::init(argc, &argv, "car_control");
		// ros::NodeHandle n;
		// this->pn = &n;
	}
	CarController(ros::NodeHandle *ptr_n){
		this->pn = ptr_n;
		//发布者<消息类型>（话题名称，消息队列长度）
		this->pub_steerL  = pn->advertise<std_msgs::Float64>("/smart/front_left_steering_position_controller/command", 100);
		this->pub_steerR  = pn->advertise<std_msgs::Float64>("/smart/front_right_steering_position_controller/command", 100);
		this->pub_rearL  = pn->advertise<std_msgs::Float64>("/smart/rear_left_velocity_controller/command", 100);
		this->pub_rearR  = pn->advertise<std_msgs::Float64>("/smart/rear_right_velocity_controller/command", 100);
		this->odom_pub = pn->advertise<nav_msgs::Odometry>("odom", 100);
		
		this->cmd_vel_sub = pn->subscribe("/smart/cmd_vel", 100, &CarController::on_cmd_vel, this);
		this->odom_sub = pn->subscribe("/smart/velocity", 100, &CarController::odom_cb, this);
	}
	virtual ~CarController();

private:
	ros::NodeHandle *pn;

	ros::Publisher pub_steerL;
	ros::Publisher pub_steerR;
	ros::Publisher pub_rearL;
	ros::Publisher pub_rearR;
	ros::Publisher odom_pub; //里程计发布者

	ros::Subscriber cmd_vel_sub;
	ros::Subscriber odom_sub;

	//void (CarController::*pCallback)() = &CarController::callback;
	double x = 0;	//后轮输出速度
	double z = 0;   //前轮转角角度
	double L = 1.57213; //轴距
	double T_front = 1.29; //前轮轮距
	double T_rear = 1.29; //后轮轮距

	//时长200ms
	ros::Duration timeout = ros::Duration(0, 200000000);
	//收到上一则消息的时间戳
	ros::Time lastMsg = ros::Time::now();
	//前轮最大转向角度
	double maxsteerInside = 0.523599;
	//最大转弯半径
	double rMax = this->L/tan(this->maxsteerInside);
	//车辆中轴线的最大转弯半径
	double rIdeal = rMax+(this->T_front/2.0);
	//车辆中轴线的最大前轮转向角度
	double maxsteer = atan(this->L/this->rIdeal);
	//用于里程计的时间积分
	ros::Time current_odom_time = ros::Time::now();
  	ros::Time last_odom_time = ros::Time::now();
	//odom里程积累
	double odom_x = 0;
	double odom_y = 0;
	double odom_th = 0;

public:
	void publish();
	void on_cmd_vel(const geometry_msgs::Twist &msg);
	void update_odom();
	//void odom_cb(const geometry_msgs::PoseStamped &msg);
	void odom_cb(const geometry_msgs::TwistStamped &msg);
	
};

#endif //CAR_CONTROLLER_H